#include "eye_hand.h"
EyeHand::EyeHand()
{
}

EyeHand::~EyeHand()
{
  
}

Mat EyeHand::Vector2Matrix()
{
    Mat warpMat;
    for (size_t i = 0; i < m_imgPoints.size(); i++)
    {
       cout<<"请输入第 " +to_string(i+1) + " 个Robot x,y坐标："<<endl;
       Point2f p;
       scanf("%f,%f",&p.x,&p.y);
       //cin >> p.x >> p.y;
       m_robotPoints.push_back(p);
       
    }
    warpMat = estimateAffine2D(m_imgPoints, m_robotPoints);
    // warpMat = estimateRigidTransform(m_imgPoints, m_robotPoints, true);
    cout<< warpMat <<endl;
try
{
    double A = warpMat.ptr<double>(0)[0];
    double B = warpMat.ptr<double>(0)[1];
    double C = warpMat.ptr<double>(0)[2];
    double D = warpMat.ptr<double>(1)[0];
    double E = warpMat.ptr<double>(1)[1];
    double F = warpMat.ptr<double>(1)[2];
    ofstream out("./matrix.dat",ios_base::out);
    if (out.is_open())
    {
        out << A << endl;
        out << B << endl;
        out << C << endl;
        out << D << endl;
        out << E << endl;
        out << F << endl;
        out.close();
    }
    }
catch(std::exception &e)
{
cout<<e.what()<<endl;
}
    return warpMat;
}
Point2f EyeHand::RmsError(Mat warrMatrix)
{
    double A = warrMatrix.ptr<double>(0)[0];
    double B = warrMatrix.ptr<double>(0)[1];
    double C = warrMatrix.ptr<double>(0)[2];
    double D = warrMatrix.ptr<double>(1)[0];
    double E = warrMatrix.ptr<double>(1)[1];
    double F = warrMatrix.ptr<double>(1)[2];
     //RMS 标定偏差
    std::vector<Point2f> points_Calc;
    double sumX = 0, sumY = 0;
    for (int i = 0;i < m_imgPoints.size();i++)
    {
        Point2f pt;
        pt.x = A * m_imgPoints[i].x + B * m_imgPoints[i].y + C;
        pt.y = D * m_imgPoints[i].x + E * m_imgPoints[i].y + F;
        points_Calc.push_back(pt);
        sumX += pow(m_robotPoints[i].x - points_Calc[i].x, 2);
        sumY += pow(m_robotPoints[i].y - points_Calc[i].y, 2);
    }
    double rmsX, rmsY;
    rmsX = sqrt(sumX / m_imgPoints.size());
    rmsY = sqrt(sumY / m_imgPoints.size());
    return Point2f(rmsX,rmsY);
}

void EyeHand::PrintLog(string strLog)
{
    cout << "*******************log*****************" << endl;
    cout << strLog << endl;
    cout << "***************************************" << endl;
}
